/* AUTHOR -> ANATOLIJS BOGDANOVS */

#include "wall_follow.h"

//#include "SERIALAPI.h"
#include "prox_sensors.h"

/* Private defines -------------------------------------------------------- */
#define     NORM            0x1A
#define     LOW             0x10
#define     STOP            0x00
#define     MAX_SPEED       0x2C 
#define     DEC_STEP        0x3

#define     PREDEF_TARGET   23
#define     MAX_D           60
#define     C               1.61
#define     d_              4
#define     THRESHOLD       1
#define     FUV             1.1
#define     FUR             1 / 1.1


/* Private flags ---------------------------------------------------------- */
uint8_t wall_follow_init = 0;

/* Private globals -------------------------------------------------------- */

uint8_t M1, M2;
int8_t FRONT, REAR, SIDE;
uint8_t TARGET = 0;
int8_t flag;
int8_t LF, LR, RF, RR;
double DISTANCE, d;

/* Init functions --------------------------------------------------------- */
// sets all preferences flags and variables of the module to initial values
void init_wall_follow(uint8_t t_distance)
{
    if (wall_follow_init)
        return;
        
    init_proximity_sensors();
    
    M1 = NORM;
    M2 = NORM;
    flag = 0;
    wall_follow_init = 1;
    if (t_distance)
        set_distance(t_distance);
    else
        set_distance(PREDEF_TARGET);
    
    return;
}

// resets all the values and preferences set in the module
void reset_wall_follow(uint8_t t_distance)
{
    init_wall_follow(t_distance);
    
    return;
}

// deinits the module
void deinit_wall_follow(void)
{
    M1 = STOP;
    M2 = STOP;
    wall_follow_init = 0;
    
    return;
}

/* Private functions ------------------------------------------------------ */
// Limits the motor speeds to some values so motors do not go at high speeds
void limit_motor_speed(void)
{
    if (M1 < 0)
        M1 = STOP;
    else if (M1 > MAX_SPEED)
        M1 = MAX_SPEED;
    if (M2 < 0)
        M2 = STOP;
    else if (M2 > MAX_SPEED)
        M2 = MAX_SPEED;
    
    return;
}

// Reads all IR the sensors
void read_sensors(void)
{
    LF = read_sensor_distance(LF_SENSOR);
    LR = read_sensor_distance(LR_SENSOR);
    RF = read_sensor_distance(RF_SENSOR);
    RR = read_sensor_distance(RR_SENSOR);
    
    return;
}

// Decides side best side possible for the current possiton the robot is at
// unless the side prefference is set by the user
int8_t decide_side(int8_t side_preference)
{
    if (side_preference)
        return side_preference;
    if (LF > MAX_D && LR > MAX_D && RF > MAX_D && RR > MAX_D)
        return 0;
    if (LF < RF && LR < RR)
        return -1;
    if (RF < LF && RR < LR)
        return 1;
    if (LF + LR < RF + RR)
        return -1;
    return 1;
}

// translates the chosen side to pick the needed values from the sensor
void init_side()
{
    if (SIDE == 1)
    {
        FRONT = RF;
        REAR  = RR;
    }
    else
    {
        FRONT = LF;
        REAR  = LR;
    }
    
    return;
}

// approximates the distance of the robot towards the wall
// angle is not been used to avoid math.h library calls
// that add a lot of 'unnecessary' computational time
void evaluate_distance(void)
{
    if (FRONT < REAR)
        DISTANCE = FRONT;
    else
        DISTANCE = REAR;
        
    return;
}

/* Public procedures ------------------------------------------------------ */
void set_distance(uint8_t t_distance)
{
    if (t_distance > 10 && t_distance < 50)
        TARGET = t_distance;
    return;
}

void evaluate_step(int8_t side_prefference, uint8_t distance)
{
    if (!wall_follow_init)
        return;
    
    set_distance(distance);
    read_sensors();
    SIDE = decide_side(side_prefference);
    init_side();
    evaluate_distance();
    // take the difference between two readings
    // approximates pololu's angle towards the wall
    // d = 4.5 -> ~45°
    d = REAR - FRONT;
    // slow down if bump front
    if (flag == 2)
    {
        flag = 1;
    }
    else if (flag == 1)
    {
        M2 = NORM;
        M1 = NORM;
        flag = 0;
    }
    // bump front detection
    if (!read_front_sensor())
    {
        if (M2 < DEC_STEP + 1)
            M2 = 0;
        else
            M2 -= DEC_STEP;
        if (M1 < DEC_STEP + 1)
            M1 = 0;
        else
            M1 -= DEC_STEP;
        flag = 2;
    }
    // no wall detected
    else if (!SIDE)
    {
        M1 = NORM;
        M2 = NORM;
    }
    // evaluate motor speeds (aka rate) at which pololu should turn
    // is determined by the state of the sensors towards the wall
    else if (d <= -d_)
    {
        M1 = (uint8_t)((NORM * (double)-d / d_) + M1) / 2;
        M2 = (uint8_t)((NORM * (double)d_ / -d) + M2) / 2;
    }
    else if (d > -d_ && d < 0)
    {
        if (DISTANCE / TARGET > C)
        {
            M1 = (uint8_t)((NORM * (double)d_ / -d) + M1) / 2;
            M2 = (uint8_t)((NORM * (double)-d / d_) + M2) / 2;
        }
        else
        {
            M1 = (uint8_t)((NORM * (1 + (double)-d / d_)) + M1) / 2;
            M2 = (NORM + M2) / 2;
        }
    }
    else if (d == 0)
    {
        if (DISTANCE < TARGET - THRESHOLD)
        {
            M1 = (uint8_t)((NORM * (double)DISTANCE / TARGET * FUV) + M1) / 2;
            M2 = (uint8_t)((NORM * (double)TARGET / DISTANCE * FUR) + M2) / 2;
        }
        else if (DISTANCE >= TARGET - THRESHOLD
            && DISTANCE <= TARGET + THRESHOLD)
        {
            M1 = (NORM + M1) / 2;
            M2 = (NORM + M2) / 2;
        }
        else if (DISTANCE / TARGET < C)
        {
            M1 = (uint8_t)((NORM * (double)DISTANCE / TARGET) + M1) / 2;
            M2 = (NORM + M2) / 2;
        }
        else
        {
            M1 = (uint8_t)((NORM * (double)DISTANCE / C) + M1) / 2;
            M2 = (NORM + M2) / 2;
        }
    }
    else if (d > 0)
    {
        if (d < d_)
        {
            if (DISTANCE < TARGET)
            {
                M1 = (uint8_t)((NORM * DISTANCE *
                    (double)d / d_ / TARGET) + M1) / 2;
                M2 = (uint8_t)((NORM * TARGET * 
                    (double)d_ / d / DISTANCE) + M2) / 2;
            }
            else if (DISTANCE == TARGET)
            {
                M1 = (uint8_t)((NORM * (1 - (double)d / 9)) + M1) / 2;
                M2 = (uint8_t)((NORM * (1 + (double)d / 9)) + M2) / 2;
            }
            else if (DISTANCE / TARGET < C)
            {
                M1 = (NORM + M1) / 2;
                M2 = (uint8_t)((NORM * (double)DISTANCE / TARGET) + M2) / 2;
            }
            else
            {
                M1 = (uint8_t)((NORM * (1 + (double)DISTANCE / C)) + M1) / 2;
                M2 = (NORM + M2) / 2;
            }
        }
        else if (d == d_)
        {
            if (DISTANCE < TARGET)
            {
                M1 = (uint8_t)((NORM * (double)DISTANCE / TARGET) + M1) / 2;
                M2 = (uint8_t)((NORM * (double)TARGET / DISTANCE) + M2) / 2;
            }
            else if (DISTANCE == TARGET)
            {
                M1 = (NORM + M1) / 2;
                M2 = (NORM + M2) / 2;
            }
            else if (DISTANCE / TARGET < C)
            {
                M1 = (NORM + M1) / 2;
                M2 = (uint8_t)((NORM * (double)DISTANCE / TARGET) + M2) / 2;
            }
            else
            {
                M1 = (NORM + M1) / 2;
                M2 = (NORM + M2) / 2;
            }
        }
        else
        {
            M1 = (uint8_t)((NORM * (double)d_ / d) + M1) / 2;
            M2 = (uint8_t)((NORM * (double)d / d_) + M2) / 2;
        }
        
    }
    // erronous state
    // should not be here as all the previous states cover all the possibilities
    else
    {
        M1 = LOW;
        M2 = LOW;
        SIDE = 0;
    }
    if (SIDE == -1)
    {
        uint16_t temp = M1;
        M1 = M2;
        M2 = temp;
    }
    limit_motor_speed();
    
    return;
}

/* Data accessors --------------------------------------------------------- */
uint8_t get_m1(void)
{
    return M1;
}

uint8_t get_m2(void)
{
    return M2;
}

// Returns front IR sensor reading towards the wall followed
uint8_t get_front_reading(void)
{
    return FRONT;
}

// Returns rear IR sensor reading towards the wall followed
uint8_t get_rear_reading(void)
{
    return REAR;
}
